# Find Rects Example
#
# 这个例子展示了如何使用april标签代码中的四元检测代码在图像中找到矩形。 四元检测算法以非常稳健的方式检测矩形，并且比基于Hough变换的方法好得多。 例如，即使镜头失真导致这些矩形看起来弯曲，它仍然可以检测到矩形。 圆角矩形是没有问题的！
# (但是，这个代码也会检测小半径的圆)...

import sensor, image, time
from machine import UART
from machine import LED

sensor.reset()
sensor.set_pixformat(sensor.RGB565) # 灰度更快(160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)
sensor.set_auto_exposure(False) # 关闭自动曝光
sensor.set_auto_gain(False)     # 关闭自动增益
sensor.skip_frames(time = 1000)

uart = UART(3, 9600)              # UART(3)P4-TX P5-RX
uart.init(9600,bits=8,parity=None,stop=1)
clock = time.clock()
led = LED("LED_BLUE")
led.on()
#******************************************************
define_find_one_point = 1   # 开关，用来设置是否只查找一个光点就确认红外激光点，为0则根据周围是否存在第二个光点在确定是激光点
red_threshold = [(66, 75, 0, 16, -2, 10)]         # 红点LAB阈值,(62, 100, 24, 52, -8, 7)
green_threshold = [(90, 100, -10, 10, -10, 6)]      # 绿点LAB阈值
red_point = [0,0]           # 存储红点坐标
#******************************************************


# 计算两点距离
def point_distance(a,b):
    return (abs(a[0]-b[0])**2 + abs(a[1]-b[1])**2)**0.5

# 查找中心方框
rect = 0
def find_center_rect(img):
    global rect
    megnitude_max = 0
    for i in img:
        rect_center_x = i.w() /2 + i.x()
        rect_center_y = i.h() /2 + i.y()
        # 计算矩形中心点与图像中心点的距离
        distance_to_center = (abs(160//2 - rect_center_x)**2 + abs(120//2 - rect_center_y)**2)**0.5
        # 计算矩形左上角点与图像中心点的距离
        distance_to_point = (abs(i.x() - rect_center_x)**2 + abs(i.y() - rect_center_y)**2)**0.5
        if distance_to_center < 40 and i.magnitude() > megnitude_max and distance_to_point < 60:
            megnitude_max = i.magnitude()
            rect = i
    return rect

# 根据RGB值查找光点（适合光点小的）
def find_dots_rgb(rect):
    point_ = [0,0]
    back_point = list()
    if rect != 0:
        for x in range(rect.x(), rect.x()+rect.w()):
            for y in range(rect.y(), rect.y()+rect.h()):
                if img.get_pixel(x,y)[0] > 250:
                    point_[0] = x+1
                    point_[1] = y+1
                    return point_
                # 当光在黑线上时R值只有230左右，可以先备份下来当全都找不到点后在从小的里面找一个最大的，同时需要注意列表不能存太多，会内存溢出
                elif img.get_pixel(x,y)[0] > 200 and len(back_point) < 50:
                    back_point.append([x,y,img.get_pixel(x,y)[0]])
    if len(back_point) > 0:
        return sorted(back_point, key=lambda x: x[2], reverse=True)[0][0:2] # 按照RGB值排序
    return point_

def find_dots_rgb_ROI(img,rect,ROI):
    point_ = [0,0]
    back_point = list()
    if rect != 0:
        for x in range(ROI[0], ROI[0]+ROI[2]):
            for y in range(ROI[1], ROI[1]+ROI[3]):
                if x < rect.x() and x > rect.x()+rect.w() and y < rect.y() and y > rect.y()+rect.h():
                    if img.get_pixel(x,y)[0] > 250:
                        point_[0] = x+1
                        point_[1] = y+1
                        return point_
                else :
                    if img.get_pixel(x,y)[0] > 250:
                        point_[0] = x+1
                        point_[1] = y+1
                        return point_
                    # 当光在黑线上时R值只有230左右，可以先备份下来当全都找不到点后在从小的里面找一个最大的，同时需要注意列表不能存太多，会内存溢出
                    elif img.get_pixel(x,y)[0] > 200 and len(back_point) < 50:
                        back_point.append([x,y,img.get_pixel(x,y)[0]])
    if len(back_point) > 0:
        return sorted(back_point, key=lambda x: x[2], reverse=True)[0][0:2] # 按照RGB值排序
    return point_

# 根据LAB查找光点
def find_dots_lab(rect,red):
    area = 0
    point_ = [0,0]
    back_point = list()
    for r in red:
        if r.area() > area and r.area() < 120:
            area = r.area()
            point_[0] = r.cx()
            point_[1] = r.cy()
    # 没有找到光点在继续从方框中以RGB扫描（用于扫描被黑线吃掉的光点）
    if area  == 0:
        return find_dots_rgb(rect)
    else :
        return point_

# 根据方框更新ROI区域
def Update_ROI():
    global rect
    if rect != 0:
        if abs(rect.x()-15) > 0 and abs(rect.y()-15) > 0 :
            dis = 15
            return [rect.x()-dis,rect.y()-dis,rect.w() + 2*dis,rect.h() + 2*dis]
    return [0,0,160,120]

# 发送坐标
def Send_coordinate(rect):
    coor = [(0,0),(0,0),(0,0),(0,0)]
    data = [0] * 13
    data[0]  = 0xa0
    data[1]  = 0xb0
    data[12] = 0xc0
    # 保存方框四个角坐标
    l = 9
    if rect != 0:
        for i in rect.corners():    # corners返回的坐标从左下角开始逆时针返回
            data[l]   = i[1]
            data[l-1] = i[0]
            l -= 2
    # 保存圆点中心坐标
    if red_point[0] != 0 and red_point[1] != 0:
        data[10] = red_point[0]
        data[11] = red_point[1]
    # 串口发送数据
    print(data)
    uart.write(bytearray(data))
    time.sleep(0.01)

# 画图
def draw(img,rect,ROI):
    if rect != 0:
        img.draw_rectangle(rect.rect(), color = (255, 0, 0))
        img.draw_rectangle(ROI, color = (255, 0, 0))
        for p in rect.corners():
            img.draw_string (p[0], p[1], str(p[0])+','+str(p[1]), color=(255, 255, 255),mono_space=False)
    if red_point[0] != 0 and red_point[1] != 0:
        img.draw_circle(red_point[0], red_point[1], 3, color = (255, 0, 0))
        img.draw_string(red_point[0], red_point[1]+5, str(red_point[0])+','+str(red_point[1]), color=(255, 255, 255),mono_space=False)


while(True):
    clock.tick()
    img = sensor.snapshot()

    rect = find_center_rect(img.find_rects(threshold = 50000))      # 查找方框
    ROI_ = Update_ROI()
    #red_point = find_dots_lab(rect,img.find_blobs(red_threshold,roi=ROI_,pixel_threshold=2))   # 查找红光点
    red_point = find_dots_rgb_ROI(img,rect,ROI_)
    Send_coordinate(rect)                                           # 发送四个点坐标给MCU
    draw(img,rect,ROI_)                                                  # 画图

    #print("fps：" + str(clock.fps()))



